#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <sensor_msgs/Range.h>
#include <angles/angles.h>
#include <ros/ros.h>
#include <future>
#include <atomic>
#include "savelog.h"

#include <movemotor_presisecontrol/TestGoLine.h>
#include <movemotor_presisecontrol/StopGoLine.h>
#include <movemotor_presisecontrol/CheckGoLine.h>
#include <movemotor_presisecontrol/StartRotate.h>
#include <movemotor_presisecontrol/StopRotate.h>
#include <movemotor_presisecontrol/CheckRotate.h>

ros::Publisher cmd_vel_pub;
std::future<void> g_control_future;
double global_x = 0,global_y = 0, global_yaw = 0;

std::atomic<bool> going_line;
std::atomic<bool> rotating;

std::atomic<bool> rotating_avoid;
bool rotating_avoid_sleep_flag;
bool ultrasonic_trigger_flag[8];

void Pub_V(double v,double w)
{
    geometry_msgs::Twist msg;
    msg.linear.x = v;
    msg.linear.y = 0;
    msg.linear.z = 0;
    msg.angular.x = 0;
    msg.angular.y = 0;
    msg.angular.z = w;

    TIC_LOG("topic cmd_val x = %f z = %f",msg.linear.x, msg.angular.z);
    cmd_vel_pub.publish(msg);
}


void OdomCallback(const nav_msgs::Odometry::ConstPtr msg)
{
    global_x = msg->pose.pose.position.x;
    global_y = msg->pose.pose.position.y;
    global_yaw = tf::getYaw(msg->pose.pose.orientation);
}


#define ULTRASONIC_TRIGGER_DIS (0.40)

//注意防止重复变量冲突，使用多个回调函数
void Ultrasound1Callback(const sensor_msgs::Range::ConstPtr msg)
{
  ultrasonic_trigger_flag[0] = ((rotating_avoid_sleep_flag) && (msg->range < ULTRASONIC_TRIGGER_DIS)) ? true : false;
}

void Ultrasound2Callback(const sensor_msgs::Range::ConstPtr msg)
{
  ultrasonic_trigger_flag[1] = ((rotating_avoid_sleep_flag) && (msg->range < ULTRASONIC_TRIGGER_DIS)) ? true : false;
}

void Ultrasound5Callback(const sensor_msgs::Range::ConstPtr msg)
{
  ultrasonic_trigger_flag[4] = ((!rotating_avoid_sleep_flag) && (msg->range < ULTRASONIC_TRIGGER_DIS)) ? true : false;
}

void Ultrasound6Callback(const sensor_msgs::Range::ConstPtr msg)
{
  ultrasonic_trigger_flag[5] = ((!rotating_avoid_sleep_flag) && (msg->range < ULTRASONIC_TRIGGER_DIS)) ? true : false;
}

enum rotate_state
{
    init_action,
    finsh_action,
    start_action
};

bool StartGoLine(movemotor_presisecontrol::TestGoLine::Request &req,
                movemotor_presisecontrol::TestGoLine::Response &res)
{
    auto go_line = [&](double distance, double speed)
    {
        going_line = true;

        double current_x = global_x, current_y = global_y;
        double accu_dis = 0.0;
        ros::Rate r(10);

        while(going_line && ros::ok()){
            accu_dis += hypot(global_x-current_x,global_y-current_y); //计算已经运行的距离
            current_x = global_x, current_y = global_y;
            if((accu_dis - distance) > DBL_EPSILON){
                break;
            }
            Pub_V(speed,0);
            r.sleep();
        }
    going_line = false;
    };

    if (rotating || going_line) return false;

    if (!g_control_future.valid()||std::future_status::ready == g_control_future.wait_for(std::chrono::duration<double>(0.0)))
    {
        g_control_future = std::async(std::launch::async, go_line, req.distance, req.speed);
    }
    else
    {
        return false;
    }

    return true;
}


bool StopGoLine(movemotor_presisecontrol::StopGoLine::Request &req,
                movemotor_presisecontrol::StopGoLine::Response &res)
{
    going_line = false;
    return true;
}

bool CheckGoLine(movemotor_presisecontrol::CheckGoLine::Request &req,
                movemotor_presisecontrol::CheckGoLine::Response &res)
{
  res.isFinished.data = !going_line;
  return true;
}


bool StartRotate(movemotor_presisecontrol::StartRotate::Request &req,
                movemotor_presisecontrol::StartRotate::Response &res)
{
   auto get_sign = [](double value) {
    return value >= 0 ? 1 : -1;
  };
  auto start_rotate = [&](double angle, double speed) {
    rotating = true;
   double global_yaw_1;

    if (fabs(speed) > 0.8)
      speed = get_sign(speed) * 0.8;

    double start_yaw = global_yaw;

    int times = ceil(angle / 180.0);
    if (angle < 0)
      times = abs(floor(angle / 180.0));

    ros::Rate r(10);
    for (int i = 1; i <= times; ++i) {
      double goal_yaw = angles::normalize_angle(start_yaw + i * M_PI);

      if (i == times)
        goal_yaw = angles::normalize_angle(start_yaw + angle / 180.0 * M_PI);

      while (rotating && ros::ok()) {
        global_yaw_1 = global_yaw;
        if(global_yaw < 0)
           global_yaw_1 = global_yaw + 2 * M_PI;
       // std::cout<< "global_yaw:" << global_yaw <<std::endl;
       // std::cout<< "global_yaw_1:  "<< global_yaw_1 <<std::endl;
       if (fabs(angles::shortest_angular_distance(global_yaw_1, goal_yaw)) < fabs(speed) / 0.3 * 0.05)
          break;
        Pub_V(0, speed);
        ROS_INFO("[MOTOR CONTROLLER] publish vel: %lf %lf, yaw: %lf goal: %lf",
                0.0, speed, global_yaw, goal_yaw);
        r.sleep();
      }
    }
    Pub_V(0, 0);
    ROS_INFO("[MOTOR CONTROLLER] publish vel: 0 0, yaw: %lf goal: %lf",
            global_yaw, angles::normalize_angle(
                start_yaw + angle / 180.0 * M_PI));
    rotating = false;
  };
  ROS_INFO("[MOTOR CONTROLLER] receive StartRotate, angle: %lf, speed: %lf",
          req.angle, req.speed);
  if (rotating || going_line ||
      fabs(req.speed) < std::numeric_limits<double>::min())
    return false;
  if (!g_control_future.valid() ||
      std::future_status::ready == g_control_future.wait_for(
          std::chrono::duration<double>(0.0))) {
    g_control_future = std::async(
        std::launch::async, start_rotate, req.angle, req.speed);
  } else {
    return false;
  }
  return true;
}

bool StopRotate(movemotor_presisecontrol::StopRotate::Request &req,
                movemotor_presisecontrol::StopRotate::Response &res)
{
    rotating = false;
    return true;
}

bool CheckRotate(movemotor_presisecontrol::CheckRotate::Request &req,
                movemotor_presisecontrol::CheckRotate::Response &res)
{
    ROS_INFO("receive CheckRotate, ret: %d", !rotating);
    res.isFinished = !rotating;
    return true;
}

bool StartRotateAvoid(movemotor_presisecontrol::StartRotate::Request &req,
                movemotor_presisecontrol::StartRotate::Response &res)
{
   auto get_sign = [](double value) {
    return value >= 0 ? 1 : -1;
  };
  auto start_rotate = [&](double angle, double speed) {
    rotating_avoid = true;
   double global_yaw_1;

    if (fabs(speed) > 0.8)
      speed = get_sign(speed) * 0.8;

    rotating_avoid_sleep_flag = speed > 0.0 ? true : false;

    double start_yaw = global_yaw;

    int times = ceil(angle / 180.0);
    if (angle < 0)
      times = abs(floor(angle / 180.0));

    ros::Rate r(10);
    for (int i = 1; i <= times; ++i) {
      double goal_yaw = angles::normalize_angle(start_yaw + i * M_PI);

      if (i == times)
        goal_yaw = angles::normalize_angle(start_yaw + angle / 180.0 * M_PI);

      while (rotating_avoid && ros::ok()) {
        global_yaw_1 = global_yaw;
        if(global_yaw < 0)
           global_yaw_1 = global_yaw + 2 * M_PI;
       // std::cout<< "global_yaw:" << global_yaw <<std::endl;
       // std::cout<< "global_yaw_1:  "<< global_yaw_1 <<std::endl;
        if (fabs(angles::shortest_angular_distance(global_yaw_1, goal_yaw)) < fabs(speed) / 0.3 * 0.05)
          break;

        //注意防止重复变量冲突，使用多个回调函数
        if(ultrasonic_trigger_flag[0] ||
        ultrasonic_trigger_flag[1] ||
        ultrasonic_trigger_flag[2] ||
        ultrasonic_trigger_flag[3] ||
        ultrasonic_trigger_flag[4] ||
        ultrasonic_trigger_flag[5] ||
        ultrasonic_trigger_flag[6] ||
        ultrasonic_trigger_flag[7])
        {
          ROS_WARN("Ultrasonic trigger");
          Pub_V(0, 0);
        }
        else
        {
          Pub_V(0, speed);
        }
        ROS_INFO("[MOTOR CONTROLLER] publish vel: %lf %lf, yaw: %lf goal: %lf",
                0.0, speed, global_yaw, goal_yaw);
        r.sleep();
      }
    }
    Pub_V(0, 0);
    ROS_INFO("[MOTOR CONTROLLER] publish vel: 0 0, yaw: %lf goal: %lf",
            global_yaw, angles::normalize_angle(
                start_yaw + angle / 180.0 * M_PI));
    rotating_avoid = false;
  };
  ROS_INFO("[MOTOR CONTROLLER] receive StartRotate, angle: %lf, speed: %lf",
          req.angle, req.speed);
  if (rotating_avoid || going_line ||
      fabs(req.speed) < std::numeric_limits<double>::min())
    return false;
  if (!g_control_future.valid() ||
      std::future_status::ready == g_control_future.wait_for(
          std::chrono::duration<double>(0.0))) {
    g_control_future = std::async(
        std::launch::async, start_rotate, req.angle, req.speed);
  } else {
    return false;
  }
  return true;
}

bool StopRotateAvoid(movemotor_presisecontrol::StopRotate::Request &req,
                movemotor_presisecontrol::StopRotate::Response &res)
{
    rotating_avoid = false;
    return true;
}

bool CheckRotateAvoid(movemotor_presisecontrol::CheckRotate::Request &req,
                movemotor_presisecontrol::CheckRotate::Response &res)
{
    ROS_INFO("receive CheckRotate, ret: %d", !rotating_avoid);
    res.isFinished = !rotating_avoid;
    return true;
}

int main(int argc,char** argv){

    std::string version = "V1.3";
    if('v' == getopt(argc,argv, "v")) std::cout << __DATE__ << "\r\n" << __TIME__ << "\r\n" <<"Revision:" << version << std::endl, exit(0);
    std::cout << "\033[32m" <<"[movemotor_presisecontrol] " << "Revision:" << version << "\033[0m" <<std::endl;

    ros::init(argc, argv, "movemotor_presisecontrol");
    going_line = false;
    rotating = false;
    rotating_avoid = false;

    for(int i=0;i<8;i++) ultrasonic_trigger_flag[i] = false;

    ros::NodeHandle nh;

    //topic
    cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",1);
    ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>("/robot/odom",1,OdomCallback);

    //注意防止重复变量冲突，使用多个回调函数
    ros::Subscriber ultrasonic_sub[8];
    ultrasonic_sub[0] = nh.subscribe<sensor_msgs::Range>("Ultrasound1",40,Ultrasound1Callback);
    ultrasonic_sub[1] = nh.subscribe<sensor_msgs::Range>("Ultrasound2",40,Ultrasound2Callback);
    ultrasonic_sub[4] = nh.subscribe<sensor_msgs::Range>("Ultrasound5",40,Ultrasound5Callback);
    ultrasonic_sub[5] = nh.subscribe<sensor_msgs::Range>("Ultrasound6",40,Ultrasound6Callback);

    //server
    ros::ServiceServer start_go_line_srv = nh.advertiseService("/device/start_go_line",StartGoLine);
    ros::ServiceServer stop_go_line_srv = nh.advertiseService("/device/stop_go_line",StopGoLine);
    ros::ServiceServer check_go_line_srv = nh.advertiseService("/device/check_go_line", CheckGoLine);
    ros::ServiceServer start_rotate_srv = nh.advertiseService("/device/start_rotate",StartRotate);
    ros::ServiceServer stop_rotate_srv = nh.advertiseService("/device/stop_rotate",StopRotate);
    ros::ServiceServer check_rotate_srv = nh.advertiseService("/device/check_rotate", CheckRotate);

    ros::ServiceServer start_rotate_srv_avoid = nh.advertiseService("/device/start_rotate_avoid",StartRotateAvoid);
    ros::ServiceServer stop_rotate_srv_avoid = nh.advertiseService("/device/stop_rotate_avoid",StopRotateAvoid);
    ros::ServiceServer check_rotate_srv_avoid = nh.advertiseService("/device/check_rotate_avoid", CheckRotateAvoid);

    ros::spin();

    going_line = false;
    rotating = false;
    rotating_avoid = false;
    if (g_control_future.valid()) g_control_future.wait();

    return 0;
}
